#include "ControlCMD.h"
#include "CAN_motor_tar.h"
#include "can.h"


Motor_Tar_Data  Set_Moto_TarData;

CAN_TX_DATA 	CAN_TxDATA;


void MotoInit(void)
{
	Set_Moto_TarData.speed[0].speed_tar = 20;
	Set_Moto_TarData.speed[1].speed_tar = 20;
	Set_Moto_TarData.speed[2].speed_tar = 20;
	Set_Moto_TarData.speed[3].speed_tar = 20;
	
	Set_Moto_TarData.speed[0].acc_tar = 20;
	Set_Moto_TarData.speed[1].acc_tar = 20;
	Set_Moto_TarData.speed[2].acc_tar = 20;
	Set_Moto_TarData.speed[3].acc_tar = 20;
	
	Set_Moto_TarData.speed[0].acc_us_tar = 20;
	Set_Moto_TarData.speed[1].acc_us_tar = 20;
	Set_Moto_TarData.speed[2].acc_us_tar = 20;
	Set_Moto_TarData.speed[3].acc_us_tar = 20;
	
	Set_Moto_TarData.speed[0].max_speed = 500;
	Set_Moto_TarData.speed[1].max_speed = 500;
	Set_Moto_TarData.speed[2].max_speed = 500;
	Set_Moto_TarData.speed[3].max_speed = 500;
	
}



uint8_t	CMDConfig = 1; 
uint16_t canid = 0;
void Control_CMD(void)
{
	
	static uint8_t moto_num = 0;
	switch (CMDConfig)
	{
		case 1:
		{
			Set_Moto_TarData.FunID = SET_TAR_SPEED_MORE;
			Set_Moto_TarData.DeviceID = SET_MOTO_1TO4;
			
			canid = ( (Set_Moto_TarData.FunID) << 5 | Set_Moto_TarData.DeviceID );
			
			CAN_TxDATA.SrcData[0] = (uint8_t)((Set_Moto_TarData.speed[0].speed_tar) >> 8);
			CAN_TxDATA.SrcData[1] = (uint8_t)(Set_Moto_TarData.speed[0].speed_tar);
			
			CAN_TxDATA.SrcData[2] = (uint8_t)((Set_Moto_TarData.speed[1].speed_tar) >> 8);
			CAN_TxDATA.SrcData[3] = (uint8_t)(Set_Moto_TarData.speed[1].speed_tar);
			
			CAN_TxDATA.SrcData[4] = (uint8_t)((Set_Moto_TarData.speed[2].speed_tar) >> 8);
			CAN_TxDATA.SrcData[5] = (uint8_t)(Set_Moto_TarData.speed[2].speed_tar);
			
			CAN_TxDATA.SrcData[6] = (uint8_t)((Set_Moto_TarData.speed[3].speed_tar) >> 8);
			CAN_TxDATA.SrcData[7] = (uint8_t)(Set_Moto_TarData.speed[3].speed_tar);
			
			CAN_SendNormalMsg(&hcan1,canid,CAN_TxDATA.SrcData,8);
			
			CMDConfig = 2;
			break;
		}
		case 2:
		{
			Set_Moto_TarData.FunID = SET_ACC_DEACC_MAXSPEED;
			switch (moto_num)
			{
				case 0:
				{
					Set_Moto_TarData.DeviceID = 0;
					
					canid = ((Set_Moto_TarData.FunID) << 5 | Set_Moto_TarData.DeviceID );
					
					CAN_TxDATA.SrcData[0] = (uint8_t)((Set_Moto_TarData.speed[0].max_speed) >> 8);
					CAN_TxDATA.SrcData[1] = (uint8_t)(Set_Moto_TarData.speed[0].max_speed);
					
					CAN_TxDATA.SrcData[2] = (uint8_t)((Set_Moto_TarData.speed[0].acc_tar) >> 8);
					CAN_TxDATA.SrcData[3] = (uint8_t)(Set_Moto_TarData.speed[0].acc_tar);
					
					CAN_TxDATA.SrcData[4] = (uint8_t)((Set_Moto_TarData.speed[0].acc_us_tar) >> 8);
					CAN_TxDATA.SrcData[5] = (uint8_t)(Set_Moto_TarData.speed[0].acc_us_tar);
					
					CAN_SendNormalMsg(&hcan1,canid,CAN_TxDATA.SrcData,6);
					
					moto_num = 1;
					break;
				}
				case 1:
				{
					Set_Moto_TarData.DeviceID = 1;
					
					canid = ( (Set_Moto_TarData.FunID) << 5 | Set_Moto_TarData.DeviceID );
					
					CAN_TxDATA.SrcData[0] = (uint8_t)((Set_Moto_TarData.speed[1].max_speed) >> 8);
					CAN_TxDATA.SrcData[1] = (uint8_t)(Set_Moto_TarData.speed[1].max_speed);
					
					CAN_TxDATA.SrcData[2] = (uint8_t)((Set_Moto_TarData.speed[1].acc_tar) >> 8);
					CAN_TxDATA.SrcData[3] = (uint8_t)(Set_Moto_TarData.speed[1].acc_tar);
					
					CAN_TxDATA.SrcData[4] = (uint8_t)((Set_Moto_TarData.speed[1].acc_us_tar) >> 8);
					CAN_TxDATA.SrcData[5] = (uint8_t)(Set_Moto_TarData.speed[1].acc_us_tar);
					
					CAN_SendNormalMsg(&hcan1,canid,CAN_TxDATA.SrcData,6);
					
					moto_num = 2;
					
	
					break;
				}
				case 2:
				{
					Set_Moto_TarData.DeviceID = 2;
					
					canid = ( (Set_Moto_TarData.FunID) << 5 | Set_Moto_TarData.DeviceID );
					
					CAN_TxDATA.SrcData[0] = (uint8_t)((Set_Moto_TarData.speed[2].max_speed) >> 8);
					CAN_TxDATA.SrcData[1] = (uint8_t)(Set_Moto_TarData.speed[2].max_speed);
					
					CAN_TxDATA.SrcData[2] = (uint8_t)((Set_Moto_TarData.speed[2].acc_tar) >> 8);
					CAN_TxDATA.SrcData[3] = (uint8_t)(Set_Moto_TarData.speed[2].acc_tar);
					
					CAN_TxDATA.SrcData[4] = (uint8_t)((Set_Moto_TarData.speed[2].acc_us_tar) >> 8);
					CAN_TxDATA.SrcData[5] = (uint8_t)(Set_Moto_TarData.speed[2].acc_us_tar);
					
					CAN_SendNormalMsg(&hcan1,canid,CAN_TxDATA.SrcData,6);
					
					moto_num = 3;
//					moto_num = 1;
//					CMDConfig = 1;
					break;
				}
				case 3:
				{
					Set_Moto_TarData.DeviceID = 3;
					
					canid = ( (Set_Moto_TarData.FunID) << 5 | Set_Moto_TarData.DeviceID );
				
					CAN_TxDATA.SrcData[0] = (uint8_t)((Set_Moto_TarData.speed[3].max_speed) >> 8);
					CAN_TxDATA.SrcData[1] = (uint8_t)(Set_Moto_TarData.speed[3].max_speed);
					
					CAN_TxDATA.SrcData[2] = (uint8_t)((Set_Moto_TarData.speed[3].acc_tar) >> 8);
					CAN_TxDATA.SrcData[3] = (uint8_t)(Set_Moto_TarData.speed[3].acc_tar);
					
					CAN_TxDATA.SrcData[4] = (uint8_t)((Set_Moto_TarData.speed[3].acc_us_tar) >> 8);
					CAN_TxDATA.SrcData[5] = (uint8_t)(Set_Moto_TarData.speed[3].acc_us_tar);
					
					CAN_SendNormalMsg(&hcan1,canid,CAN_TxDATA.SrcData,6);
					
					moto_num = 0;
					CMDConfig = 1;
					break;
				}
			}//switch (moto_num)
			
			break;
		}//case 2:
		case 3:
		{
			break;
		}
		default:
		{
			break;
		}
	
	}//switch (CMDConfig)

}

uint8_t PressKeyNum = 0;

void Send_ControlCMD(void)
{
	if (PressKeyNum == 0)
	{
		if (KEY_SEND == 0)
		{
			HAL_Delay(10);
			if (KEY_SEND == 0)
			{
				Control_CMD();
				PressKeyNum = 1;
			}
		}//if (KEY_SEND == 0)
	}
	else
	{
		if (KEY_SEND == 1)
		{
			HAL_Delay(10);
			if (KEY_SEND == 1)
			{
				PressKeyNum = 0;
			}
		}
	}


}
